/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief  用来发布以显示的功能类文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#ifndef COSTMAP_2D_COSTMAP_2D_PUBLISHER_H_
#define COSTMAP_2D_COSTMAP_2D_PUBLISHER_H_
#include <config_file.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/footprint.h>
#include <costmap_2d/msg_struct.h>
#include <geometry_msgs/PolygonStamped.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <nav_msgs/OccupancyGrid.h>
#include <ros/ros.h>

namespace costmap_2d {
/**
 *@brief 发布数据到topic类
 *@author tuyinan
 *@version 2022-04-xx
 */
class Costmap2DPublisher {
 public:
  /**
   *@brief 初始化发布类
   *@param string name 发布的topic空间名
   *@param Costmap2D* costmap 代价地图
   *@param ConfigFile* configNode 配置文件类
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  Costmap2DPublisher(std::string name, Costmap2D* costmap,
                     ConfigFile* configNode);

  /**
   *@brief 析构函数
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  ~Costmap2DPublisher();
  /**
   *@brief 更新边界数值
   *@param unsigned int x0 左上角边界的x
   *@param unsigned int xn 左上角边界的y
   *@param unsigned int y0 右下角边界的x
   *@param unsigned int yn 右下角边界的y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void updateBounds(unsigned int x0, unsigned int xn, unsigned int y0,
                    unsigned int yn) {
    x0_ = std::min(x0, x0_);
    xn_ = std::max(xn, xn_);
    y0_ = std::min(y0, y0_);
    yn_ = std::max(yn, yn_);
  }
  /**
   *@brief 发布代价地图
   *@param 无
   *@return  void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void publishCostmap();
  /**
   *@brief 发布机器人占用面积
   *@param double x 机器人位姿x坐标
   *@param double y 机器人位姿y坐标
   *@param double yaw 机器人位姿旋转角度
   *@param  std::vector<costmap_2d::Point> padded_footprint_
   *机器人占地膨胀后的坐标集合
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void publishFootprint(double x, double y, double yaw,
                        std::vector<costmap_2d::Point> padded_footprint_);

 private:
  /**
   *@brief 准备用来发布的网格信息
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void prepareGrid();
  /**
   *@brief
   *全代价地图发布器的回调函数，该回调函数会在有监听器监听这个发布的时候调用该程序
   *@param const ros::SingleSubscriberPublisher& pub 发布器
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void onNewSubscription(const ros::SingleSubscriberPublisher& pub);
  ///代价地图
  Costmap2D* costmap_;
  ///代价地图的frame id
  std::string global_frame_;
  ///代价地图边界
  unsigned int x0_, xn_, y0_, yn_;
  ///代价地图的原始开始坐标
  double saved_origin_x_, saved_origin_y_;
  ///是否只发送全代价地图
  bool always_send_full_costmap_;
  ///代价地图发布器
  ros::Publisher costmap_pub_;
  ///更新代价地图发布器
  ros::Publisher costmap_update_pub_;
  ///占位发布器
  ros::Publisher footprint_pub_;
  ///要发送的地图数据
  nav_msgs::OccupancyGrid grid_;
  ///转换表，把（0-255）的代价地图转换到（-1 - 100）
  static char* cost_translation_table_;
};
}  // namespace costmap_2d
#endif  // COSTMAP_2D_COSTMAP_2D_PUBLISHER_H
